#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <emmintrin.h>

#define N 4000000
/* #define DEBUG 0 */

typedef struct s_pixel_rgb {
  float r0, r1, r2, r3;
  float g0, g1, g2, g3;
  float b0, b1, b2, b3;
} t_pixel_rgb;

typedef struct s_pixel_yuv {
  float y0, y1, y2, y3;
  float u0, u1, u2, u3;
  float v0, v1, v2, v3;
} t_pixel_yuv;

// #define iter(i) \
//     yuv.y[i] = 0.299 * rgb.r[i] + 0.587 * rgb.g[i] + 0.114 * rgb.b[i]; \
//     yuv.u[i] = 0.492 * (rgb.b[i] - (0.299 * rgb.r[i] + 0.587 * rgb.g[i] + 0.114 * rgb.b[i])); \
//     yuv.v[i] = 0.877 * (rgb.r[i] - (0.299 * rgb.r[i] + 0.587 * rgb.g[i] + 0.114 * rgb.b[i]));

void rgb_to_yuv(t_pixel_rgb* rgb, t_pixel_yuv* yuv, int len, int N_iter)
{
  int i,j;
  __m128 r, g, b, y, u, v, t1, t2, t3, t4, c299, c587, c114, c492, c877;
  c299 = _mm_set_ps(0.299f, 0.299f, 0.299f, 0.299f);
  c587 = _mm_set_ps(0.587f, 0.587f, 0.587f, 0.587f);
  c114 = _mm_set_ps(0.114f, 0.114f, 0.114f, 0.114f);
  c492 = _mm_set_ps(0.492f, 0.492f, 0.492f, 0.492f);
  c877 = _mm_set_ps(0.877f, 0.877f, 0.877f, 0.877f);

  for (j=0; j<N_iter; j++)
  {
    for (i=0; i<len; i++) {
      r = _mm_load_ps(&(rgb[i].r0));
      t1 = _mm_mul_ps(c299, r);
      g = _mm_load_ps(&(rgb[i].g0));
      t2 = _mm_mul_ps(c587, g);
      b = _mm_load_ps(&(rgb[i].b0));
      t3 = _mm_mul_ps(c114, b);
      t4 = _mm_add_ps(t1, t2);
      y = _mm_add_ps(t4, t3);
      _mm_store_ps(&(yuv[i].y0), y);
      t1 = _mm_sub_ps(b, y);
      u = _mm_mul_ps(c492, t1);
      _mm_store_ps(&(yuv[i].u0), u);
      t2 = _mm_sub_ps(r, y);
      v = _mm_mul_ps(c877, t2);
      _mm_store_ps(&(yuv[i].v0), v);
    }
  }
}

int main (int argc, char *argv[])
{
  t_pixel_rgb* rgb; t_pixel_yuv* yuv; // Vectores para malloc

  int i, j, m;
  int n=N; // tamanyo vectores
  int NIt=50; // Num.iters

  if (argc>1) NIt = atoi(argv[1]);
  if (argc>2) n = atoi(argv[2]);

  if (n>N) { printf("Maxima longitud vector: %d\n",N); exit(-1); }

  // Assumint n % 4 == 0...
  m = n/4;

  if (posix_memalign((void**)&rgb, 16, m*sizeof(t_pixel_rgb)) !=0) {
      fprintf(stderr, "No memory.\n");
      exit(-1);
  }

  if (posix_memalign((void**)&yuv, 16, m*sizeof(t_pixel_yuv)) !=0) {
      fprintf(stderr, "No memory.\n");
      exit(-1);
  }

  i = 0;
  for (j=0;j<m;j++) {
     rgb[j].r0=(float) ((i<<26) ^ ((i + 340) << 22) ^ i ^ 0xf215fabc);
     rgb[j].g0=(float) (((i+450)<<27) ^ ((i + 567) <<20) ^ i ^ 0xb152e8ca);
     rgb[j].b0=(float) (((i+7823454) << 25) ^0xbad51cde);
     i++;
     rgb[j].r1=(float) ((i<<26) ^ ((i + 340) << 22) ^ i ^ 0xf215fabc);
     rgb[j].g1=(float) (((i+450)<<27) ^ ((i + 567) <<20) ^ i ^ 0xb152e8ca);
     rgb[j].b1=(float) (((i+7823454) << 25) ^0xbad51cde);
     i++;
     rgb[j].r2=(float) ((i<<26) ^ ((i + 340) << 22) ^ i ^ 0xf215fabc);
     rgb[j].g2=(float) (((i+450)<<27) ^ ((i + 567) <<20) ^ i ^ 0xb152e8ca);
     rgb[j].b2=(float) (((i+7823454) << 25) ^0xbad51cde);
     i++;
     rgb[j].r3=(float) ((i<<26) ^ ((i + 340) << 22) ^ i ^ 0xf215fabc);
     rgb[j].g3=(float) (((i+450)<<27) ^ ((i + 567) <<20) ^ i ^ 0xb152e8ca);
     rgb[j].b3=(float) (((i+7823454) << 25) ^0xbad51cde);
     i++;
  }

  rgb_to_yuv(rgb, yuv, m, NIt);

  // Rearrange to preserve original output
  for (i=0;i<m;i++) {
    rgb[i].r0 = yuv[i].y0;
    rgb[i].r1 = yuv[i].u0;
    rgb[i].r2 = yuv[i].v0;
    rgb[i].r3 = yuv[i].y1;
    rgb[i].g0 = yuv[i].u1;
    rgb[i].g1 = yuv[i].v1;
    rgb[i].g2 = yuv[i].y2;
    rgb[i].g3 = yuv[i].u2;
    rgb[i].b0 = yuv[i].v2;
    rgb[i].b1 = yuv[i].y3;
    rgb[i].b2 = yuv[i].u3;
    rgb[i].b3 = yuv[i].v3;
  }
  write(1, rgb, n*sizeof(t_pixel_rgb));
  return(0);
}
